#ifndef _SEARCH_C_
#define _SEARCH_C_

#include "search_algorithms.h"
#include <vector>
#include <math.h>
#include <set>
#include <utility>
#include <limits>
#include <stdlib.h>

using namespace std;

vector<vector<int> > Dijkstra(double ** Graph, int graph_size, int source){
    /*------------------------------------------------------------------------------*
    |   Inputs: pointer to first element of a square matrix of size graph_size, and |
    |           the starting node                                                   |
    |   Output: 2D vector of paths, where paths[x] returns a sequence of distance-  |
    |           optimal paths, and paths[x][y] returns y'th step on the path to x   |
    |   *** Pseudocode from: http://en.wikipedia.org/wiki/Dijkstra%27s_algorithm    |
    *------------------------------------------------------------------------------*/

    // Initializations
    /*double Graph[graph_size][graph_size]; // Square matrix of distances between nodes: inf if unconnected
    for (int i=0; i<graph_size; i++){
        for (int j=0; j<graph_size; j++){
            Graph[i][j]=graph_first[i*graph_size+j];
        }
    }
    */
    double dist[graph_size];    // Distance between nodes, initially set to infinity (unreachable)
    int previous[graph_size];   // Previous node in optimal path from source
    vector<vector<int> > paths; // Path from start node to end node

    for (int i=0; i<graph_size; i++){
        dist[i] = numeric_limits<double>::infinity();
        previous[i] = -1;
    }
    dist[source] = 0;
    set<pair<double, int> > Q;  // Set of all nodes in graph, sorted by distance to starting point
    for (int i=0; i<graph_size; i++){
        Q.insert(make_pair(dist[i], i));
    }
    while (!Q.empty()){
        int u = Q.begin()->second;  // Grabs node with the smallest distance in dist[]
        if (dist[u]==numeric_limits<double>::infinity()) {
            break; // All remaining vertices inaccessible from source
        }
        Q.erase(Q.begin());         // Erases node with smallest distance in dist

        // Iterates through neighbors of 'u'
        set<pair<double, int> >::iterator it;
        for (it=Q.begin(); it!=Q.end(); ++it){
            int v = it->second;
            double alt = dist[u] + Graph[u][v];
            if (alt<dist[v]){
                dist[v] = alt;
                previous[v] = u;
                Q.erase(it), Q.insert(make_pair(dist[v],v));
            }
        }
    }

    for (int i=0; i<graph_size; i++){
        vector<int> S;      // Empty sequence (single path)
        int u = i;
        while(previous[u]!=-1){
            S.push_back(u);
            u = previous[u];
        }
        vector<int> rS;
        for (uint j=0; j<S.size(); j++){
            rS.push_back(S[S.size()-j-1]);
        }
        paths.push_back(rS);
    }
    return paths;
}



vector<int> Dijkstra(double ** Graph, int graph_size, int source, int target){
    /*------------------------------------------------------------------------------*
    |   Inputs: pointer to first element of a square matrix of size graph_size,     |
    |           the starting node, and the destination node                         |
    |   Output: vector of hops to get to the destination node                       |
    |   *** Pseudocode from: http://en.wikipedia.org/wiki/Dijkstra%27s_algorithm    |
    *------------------------------------------------------------------------------*/

    // Initializations
    /*
    double Graph[graph_size][graph_size]; // Square graph: matrix of distances between nodes
    for (int i=0; i<graph_size; i++){
        for (int j=0; j<graph_size; j++){
            Graph[i][j]=graph_first[i*graph_size+j];
        }
    }
    */
    double dist[graph_size];    // Distance between nodes, initially set to infinity (unreachable)
    int previous[graph_size];   // Previous node in optimal path from source

    for (int i=0; i<graph_size; i++){
        dist[i] = numeric_limits<double>::infinity();
        previous[i] = -1;
    }
    dist[source] = 0;
    set<pair<double, int> > Q;      //   Set of all nodes in graph, sorted by distance to starting point
    for (int i=0; i<graph_size; i++){
        Q.insert(make_pair(dist[i], i));
    }
    while (!Q.empty()){
        int u = Q.begin()->second;  // Grabs node with the smallest distance in dist[]
        if (dist[u]==numeric_limits<double>::infinity()) {
            break;                  // All remaining vertices inaccessible from source
        }
        Q.erase(Q.begin());         // Erases node with smallest distance in dist

        // Iterates through neighbors of 'u'
        set<pair<double, int> >::iterator it;
        for (it=Q.begin(); it!=Q.end(); ++it){
            int v = it->second;
            double alt = dist[u] + Graph[u][v];
            if (alt<dist[v]){
                dist[v] = alt;
                previous[v] = u;
                Q.erase(it), Q.insert(make_pair(dist[v],v));
            }
        }
    }

    vector<int> S;      // Empty sequence (single path)
    int u=target;
    while(previous[u]!=-1){
        S.push_back(u);
        u = previous[u];
    }
    vector<int> rS;     // Reverse S
    for (uint i=0; i<S.size(); i++){
        rS.push_back(S[S.size()-i-1]);
    }
    return rS;
}
vector<vector<vector<int> > > DijkstraK(double ** Graph, int graph_size, int source, int K)
{
    //Create Lookup Table for paths.
    vector<vector<vector<int> > > table;

    for(int i = 0; i < graph_size; i++)
    {
        vector<vector<int> > temp;
        bool skip = true;
        for(int closest = 0; closest < K; closest++)
        {
            vector<pair<int, double> > oldValues;
            if(closest != 0)
            {
                if(closest >= graph_size || i == source || !skip) continue;
                for(int a = 0; a < temp[closest -1].size(); a++)
                {
                    int shortest = 0;
                    double shortestVal = Graph[source][temp[closest-1][0]];
                    for(int k = 1; k < temp[closest-1].size(); k++)
                    {
                        if(Graph[temp[closest-1][k-1]][temp[closest-1][k]] < shortestVal)
                        {
                            shortest = k;
                            shortestVal = temp[closest-1][k];
                        }
                    }
                    if(shortest == 0)
                    {
                        oldValues.push_back(make_pair(shortest, Graph[source][temp[closest-1][0]]));
                        Graph[source][temp[closest-1][0]] = 999999;
                    }
                    else
                    {
                        oldValues.push_back(make_pair(shortest, Graph[temp[closest-1][shortest-1]][temp[closest-1][shortest]]));
                        Graph[temp[closest-1][shortest-1]][temp[closest-1][shortest]] = 999999;
                    }
                }
                /*
                skip = false;
                for(unsigned int z = 0; z < temp[closest-1].size(); z++)
                {
                    if(z == 0)
                    {
                        if(Graph[source][temp[closest-1][z]] != 999999 && Graph[source][temp[closest-1][z]] != numeric_limits<double>::infinity()) skip = true;
                    }
                    else
                    {
                        if(Graph[temp[closest-1][z-1]][temp[closest-1][z]] != 999999 && Graph[temp[closest-1][z-1]][temp[closest-1][z]] != numeric_limits<double>::infinity()) skip = true;
                    }
                    if(skip) continue;
                }
                */
            }
            if(closest >= graph_size - 1 || !skip) continue;
            vector<int> temp2 = Dijkstra(Graph, graph_size, source, i);
            temp.push_back(temp2);
            for(int old = 0; old < oldValues.size(); old++)
            {
                int shortest = oldValues[old].first;
                double shortestVal = oldValues[old].second;
                if(shortest == 0)
                {
                    Graph[source][temp[closest-1][0]] = shortestVal;
                }
                else
                {
                    Graph[temp[closest-1][shortest-1]][temp[closest-1][shortest]] = shortestVal;
                }

            }
        }
        table.push_back(temp);
    }
    return table;
}


int best_i(){
    // Returns best index, given a set of constraints, vector of actions, and comparison array,
}

#endif
